import cv2, sys, numpy as np, Image

from PyQt4 import QtCore
from PyQt4 import Qt
from PyQt4 import QtGui
from PyQt4 import QtOpenGL

from OpenGL.GL import *
from OpenGL.GLU import *
from OpenGL.GLUT import *


class CameraDevice(QtCore.QObject):
    
    _DEFAULT_FPS = 30
    
    newFrame = QtCore.pyqtSignal(np.ndarray)
    
    def __init__(self, cameraId=0, mirrored=False, parent=None):
        super(CameraDevice, self).__init__(parent)
        
        self.mirrored = mirrored
        
        self._cameraDevice = cv2.VideoCapture(cameraId) 
        
        self._timer = QtCore.QTimer(self)
        self._timer.timeout.connect(self._queryFrame)
        self._timer.setInterval(1000/self.fps)
        
        self.paused = False
    
    @QtCore.pyqtSlot()
    def _queryFrame(self):
        success, frame = self._cameraDevice.read()
        if self.mirrored:
            frame = cv2.flip(frame, 1)
        if frame != None: #if added SWI
            self.newFrame.emit(frame)
        #self.newFrame.emit(frame)

    @property
    def paused(self):
        return not self._timer.isActive()
    
    @paused.setter
    def paused(self, p):
        if p:
            self._timer.stop()
        else:
            self._timer.start()
    
    @property
    def frameSize(self):
        w = self._cameraDevice.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH)
        h = self._cameraDevice.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT)
        return int(w), int(h)
    
    @property
    def fps(self):
        _fps = self._cameraDevice.get(cv2.cv.CV_CAP_PROP_FPS)
        if not _fps > 0:
            _fps = self._DEFAULT_FPS
        return _fps



class ARWidget(QtOpenGL.QGLWidget):
    
    newFrame = QtCore.pyqtSignal(np.ndarray)
    
    def __init__(self, cameraDevice, parent=None):
        super(ARWidget, self).__init__(parent)
        
        self._frame = None

        self._pose = np.eye(4, dtype = np.float64)
        
        self._cameraDevice = cameraDevice
        self._cameraDevice.newFrame.connect(self._onNewFrame)
        
        w, h = self._cameraDevice.frameSize

        if not w*h:
            w = 640
            h = 480
            raise ValueError("Incorrect image size! (An error seems to have occured with the video device)")
        
        self.setMinimumSize(w, h)
        self.setMaximumSize(w, h)

    def initializeGL(self):
        glViewport(0, 0, self.width(), self.height());
        glClearColor(1.0, 0.5, 0.0, 1.0)
        glClearDepth(1.0)
        glPolygonMode( GL_FRONT_AND_BACK, GL_FILL )
        glEnable(GL_NORMALIZE);
        glEnable(GL_DEPTH_TEST);
        glShadeModel(GL_SMOOTH);
        glDepthMask(GL_TRUE);
        glDepthFunc(GL_LEQUAL);
        glEnable(GL_LIGHT0);
        glLineWidth(3.0)


    def paintGL(self):
        if self._frame is None:
            return
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
        self.drawFrame()
        #self.draw3DScene()

    def resizeGL(self, w, h):
        pass
    
    @QtCore.pyqtSlot(np.ndarray)
    def _onNewFrame(self, frame):
        self._frame = np.copy(frame)
        self.newFrame.emit(self._frame)
        
        ### TODO: (Ignore for assignment 3)         ###
        ### Estimate the camera/marker pose         ###
        ### For example:                            ###
        
        #self._pose = tracker.estimatePose(self._frame)

        #and delete this:
        self._pose[2, 3] = (self._pose[2, 3] + 1)%100
        
        self.updateGL()

    def draw3DScene(self):
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        gluPerspective(45.0, float(self.width())/float(self.height()), 0.1, 1000.0)
        # Better: glMultMatrixd(tracker.getProjectionMatrix().T)
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
        gluLookAt(0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, -1.0, 0.0)

        # use the etimated pose for model transformation
        glMultMatrixd(self._pose.T)

        # draw simple coordinate axes
        glBegin(GL_LINES)
        glColor3d(1.0,0.0,0.0)
        glVertex3d(0.0, 0.0, 0.0)
        glVertex3d(10.0, 0.0, 0.0)
        glColor3d(0.0,1.0,0.0)
        glVertex3d(0.0, 0.0, 0.0)
        glVertex3d(0.0, 10.0, 0.0)
        glColor3d(0.0, 0.0, 1.0)
        glVertex3d(0.0, 0.0, 0.0)
        glVertex3d(0.0, 0.0, 10.0)
        glEnd()

        # draw teapot
        glEnable(GL_LIGHTING)
        glPushMatrix()
        glTranslate(0.0, 0.0, 1.0)
        glRotate(90.0, 1.0, 0.0, 0.0)
        glutSolidTeapot(1)
        glPopMatrix()
        glDisable(GL_LIGHTING)


    def drawFrame(self):
        glMatrixMode(GL_PROJECTION);
        glLoadIdentity();
        glOrtho(0.0, self.width(), self.height(), 0.0, -1.0, 1.0);
        glMatrixMode(GL_MODELVIEW);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        glLoadIdentity();

        # convert the numpy array to an opengl texture
        glTexImage2D(GL_TEXTURE_2D, 0, 3, self._frame.shape[1], self._frame.shape[0], 0, GL_BGR, GL_UNSIGNED_BYTE, self._frame.tostring());
        glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
        glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
            
        glDisable(GL_DEPTH_TEST);

        glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
        glColor3d(1.0,1.0,1.0);

        # draw the frame mapped to a textured quad
        glEnable(GL_TEXTURE_2D);
        glBegin(GL_QUADS);
        glTexCoord2f( 0.0, 0.0);
        glVertex3f( 0, 0, 0 );
        
        glTexCoord2f( 1.0, 0.0 );
        glVertex3f( self.width(), 0, 0 );
        
        glTexCoord2f( 1.0, 1.0 );
        glVertex3f( self.width(), self.height(), 0 );
        
        glTexCoord2f( 0.0, 1.0 );
        glVertex3f( 0, self.height(), 0 );
        glEnd();
        glDisable(GL_TEXTURE_2D);
            
        glEnable(GL_DEPTH_TEST);
    

    def changeEvent(self, e):
        if e.type() == QtCore.QEvent.EnabledChange:
            if self.isEnabled():
                self._cameraDevice.newFrame.connect(self._onNewFrame)
            else:
                self._cameraDevice.newFrame.disconnect(self._onNewFrame)





class MyMainWindow(QtGui.QWidget):
    def __init__(self):
        self.lastFrame = None
        self.imgGray = False
        self.imgGrayScale = False
        self.imgShowHistoEquals = False
        self.imgBin = False
        self.imgBinOtsu = False
        self.imgInvert = False
        self.imgShowHisto = False
        self.imgShowLinear = False

        QtGui.QWidget.__init__(self, None)
        self.setWindowTitle('Simple AR Display')
        
        # specify layout
        vbox = QtGui.QVBoxLayout(self)
        vbox.setDirection(QtGui.QBoxLayout.LeftToRight)
        
        # get camera device
        self.cameraDevice = CameraDevice(mirrored=False)
        self.cameraDevice.newFrame.connect(self.onNewCameraFrame)
        
        # add widget to show the augmented video input image
        arWidget = ARWidget(self.cameraDevice)
        arWidget.newFrame.connect(self.onNewCameraFrame)
        vbox.addWidget(arWidget)
        
        # add slider to control vertical position
        self.w, self.h = self.cameraDevice.frameSize
        self.verticalPositionSlider = QtGui.QSlider(QtCore.Qt.Vertical)
        self.verticalPositionSlider.setRange(1,self.h)
        self.verticalPositionSlider.setSingleStep(2);
        self.verticalPositionSlider.valueChanged[int].connect(self.verticalPositionSlidervalue)
        self.verticalPositionSlider.setValue(self.h/2)
        vbox.addWidget(self.verticalPositionSlider)

        #horizontal slider
        self.horizontalPositionSlider = QtGui.QSlider(QtCore.Qt.Vertical)
        self.horizontalPositionSlider.setRange(1,self.w)
        self.horizontalPositionSlider.setSingleStep(2);
        self.horizontalPositionSlider.valueChanged[int].connect(self.horizontalPositionSlidervalue)
        self.horizontalPositionSlider.setValue(self.w/2)
        vbox.addWidget(self.horizontalPositionSlider)

        self.sizePositionSlider = QtGui.QSlider(QtCore.Qt.Vertical)
        self.sizePositionSlider.setRange(50,500)
        self.sizePositionSlider.setSingleStep(2);
        self.sizePositionSlider.valueChanged[int].connect(self.sizePositionSlidervalue)
        self.sizePositionSlider.setValue(225)
        vbox.addWidget(self.sizePositionSlider)

        self.sizeBinSlider = QtGui.QSlider(QtCore.Qt.Vertical)
        self.sizeBinSlider.setRange(1,254)
        self.sizeBinSlider.setSingleStep(1);
        self.sizeBinSlider.valueChanged[int].connect(self.sizeBinValue)
        self.sizeBinSlider.setValue(127)
        vbox.addWidget(self.sizeBinSlider)

        self.sizeLinearSliderLow = QtGui.QSlider(QtCore.Qt.Vertical)
        self.sizeLinearSliderLow.setRange(1,254)
        self.sizeLinearSliderLow.setSingleStep(1);
        self.sizeLinearSliderLow.valueChanged[int].connect(self.sizeLinearSliderLowValue)
        self.sizeLinearSliderLow.setValue(100)
        vbox.addWidget(self.sizeLinearSliderLow)

        self.sizeLinearSliderHigh = QtGui.QSlider(QtCore.Qt.Vertical)
        self.sizeLinearSliderHigh.setRange(2,255)
        self.sizeLinearSliderHigh.setSingleStep(1);
        self.sizeLinearSliderHigh.valueChanged[int].connect(self.sizeLinearSliderHighValue)
        self.sizeLinearSliderHigh.setValue(200)
        vbox.addWidget(self.sizeLinearSliderHigh)

        self.sizeLinearSliderAlpha = QtGui.QSlider(QtCore.Qt.Vertical)
        self.sizeLinearSliderAlpha.setRange(1,254)
        self.sizeLinearSliderAlpha.setSingleStep(1);
        self.sizeLinearSliderAlpha.valueChanged[int].connect(self.sizeLinearSliderAlphaValue)
        self.sizeLinearSliderAlpha.setValue(100)
        vbox.addWidget(self.sizeLinearSliderAlpha)

        self.sizeLinearSliderBeta = QtGui.QSlider(QtCore.Qt.Vertical)
        self.sizeLinearSliderBeta.setRange(2,255)
        self.sizeLinearSliderBeta.setSingleStep(1);
        self.sizeLinearSliderBeta.valueChanged[int].connect(self.sizeLinearSliderBetaValue)
        self.sizeLinearSliderBeta.setValue(200)
        vbox.addWidget(self.sizeLinearSliderBeta)

        self.showInverse = QtGui.QPushButton('Invertieren', self)
        self.showInverse.clicked.connect(self.handleClick)
        
        self.showHistogramm = QtGui.QPushButton('Histogramm', self)
        self.showHistogramm.clicked.connect(self.handleClick)

        self.showImgGray = QtGui.QPushButton('Grau', self)
        self.showImgGray.clicked.connect(self.handleClick)

        self.showGraySpread = QtGui.QPushButton('Grau spreizen', self)
        self.showGraySpread.clicked.connect(self.handleClick)

        self.showHistoEquals = QtGui.QPushButton('Histogrammausgleich', self)
        self.showHistoEquals.clicked.connect(self.handleClick)

        self.showLinear = QtGui.QPushButton('Lineares Mapping', self)
        self.showLinear.clicked.connect(self.handleClick)

        self.showBin = QtGui.QPushButton('Binarisieren', self)
        self.showBin.clicked.connect(self.handleClick)

        self.showBinOtsu = QtGui.QPushButton('Binarisieren Otsu', self)
        self.showBinOtsu.clicked.connect(self.handleClick)

        vbox.addWidget(self.showInverse)
        vbox.addWidget(self.showHistogramm)
        vbox.addWidget(self.showImgGray)
        vbox.addWidget(self.showGraySpread)
        vbox.addWidget(self.showHistoEquals)
        vbox.addWidget(self.showLinear)
        vbox.addWidget(self.showBin)
        vbox.addWidget(self.showBinOtsu)
    
    def handleClick(self):
        sender = self.sender()
        command = sender.text()

        if command == 'Invertieren':
            self.imgInvert ^= True

        if command == 'Grau':
            self.imgGray ^= True

        if command == 'Grau spreizen':
            self.imgGrayScale ^= True
            self.imgShowHistoEquals = False
            self.imgBin = False
            self.imgBinOtsu = False

        if command == 'Histogrammausgleich':
            self.imgGrayScale = False
            self.imgShowHistoEquals ^= True
            self.imgBin = False
            self.imgBinOtsu = False

        if command == 'Histogramm':
            self.imgShowHisto ^= True

        if command == 'Binarisieren':
            self.imgGrayScale = False
            self.imgShowHistoEquals = False
            self.imgBin ^= True
            self.imgBinOtsu = False

        if command == 'Binarisieren Otsu':
            self.imgGrayScale = False
            self.imgShowHistoEquals = False
            self.imgBin = False
            self.imgBinOtsu ^= True

        if command == 'Lineares Mapping':
            self.imgGrayScale = False
            self.imgShowHistoEquals = False
            self.imgBin = False
            self.imgBinOtsu = False
            self.imgShowLinear ^= True

    def verticalPositionSlidervalue(self, value):
        self.verticalPositionvalue = value

    def horizontalPositionSlidervalue(self, value):
        self.horizontalPositionvalue = value

    def sizePositionSlidervalue(self, value):
        self.sizePositionvalue = value

    def sizeBinValue(self, value):
        self.binValue = value

    def sizeLinearSliderLowValue(self, value):
        self.linearSliderLowValue = value

    def sizeLinearSliderHighValue(self, value):
        self.linearSliderHighValue = value

    def sizeLinearSliderAlphaValue(self, value):
        self.linearSliderAlphaValue = value

    def sizeLinearSliderBetaValue(self, value):
        self.linearSliderBetaValue = value

    def onNewCameraFrame(self, frame):
        # set vertical position of rectangle multiply by 5 to get to the bottom side
        verticalPosition = int(self.verticalPositionvalue)*5
        # set horizontal position of the rectangle multiply by 7 to geht to the right side
        horizontalPosition = int(self.horizontalPositionvalue)*7
        # set size ROI window
        sizeROI = int(self.sizePositionvalue)
        #set binary value
        calcBinValue = int(self.binValue)
        linearLowValue = int(self.linearSliderLowValue)
        linearHighValue = int(self.linearSliderHighValue)
        linearAlphaValue = int(self.linearSliderAlphaValue)
        linearBetaValue = int(self.linearSliderBetaValue)

        frameCopy = frame.copy()

        # invert img
        if self.imgInvert:
            sectionInvert = frameCopy[verticalPosition:verticalPosition+sizeROI,horizontalPosition:horizontalPosition+sizeROI].copy()
            frameCopy[verticalPosition:verticalPosition+sizeROI,horizontalPosition:horizontalPosition+sizeROI] = cv2.bitwise_not(sectionInvert.copy())

        if self.imgGray:
            frameCopy = cv2.cvtColor(frameCopy, cv2.COLOR_BGR2GRAY)
            if self.imgGrayScale:
                gmin = frameCopy.min()
                gmax = frameCopy.max()
                for i in range(len(frameCopy)-1):
                    for j in range(len(frameCopy[0])-1):
                        frameCopy[i][j] = (frameCopy[i][j]-gmin) * (255/(gmax-gmin))
                #gnew = (g-gmin) * (255/(gmax-gmin))
            if self.imgShowLinear:
                for i in range(len(frameCopy)-1):
                    for j in range(len(frameCopy[0])-1):
                        if frameCopy[i][j] > linearLowValue and frameCopy[i][j] < linearHighValue:
                            frameCopy[i][j] = (linearAlphaValue * frameCopy[i][j]) + linearBetaValue
            if self.imgShowHistoEquals:
                frameCopy = cv2.equalizeHist(frameCopy)    # Remember histogram equalization works only for grayscale images
            if self.imgBin:
                _ , frameCopy  = cv2.threshold(frameCopy,calcBinValue,255,cv2.THRESH_BINARY)
            if self.imgBinOtsu:
                _ , frameCopy  = cv2.threshold(frameCopy,calcBinValue,255,cv2.THRESH_BINARY | cv2.THRESH_OTSU)
                print 'schwellwert start ', calcBinValue
            
            cv2.normalize(frameCopy,frameCopy,0,255,cv2.NORM_MINMAX)
            frameCopy = cv2.cvtColor(frameCopy, cv2.COLOR_GRAY2BGR)

        if self.imgShowHisto:
            h = np.zeros((360,360,3))
            bins = np.arange(360).reshape(360,1)
            color = [ (255,0,0),(0,255,0),(0,0,255) ]

            for ch, col in enumerate(color):
                hist_item = cv2.calcHist([frameCopy],[ch],None,[360],[0,255])
                cv2.normalize(hist_item,hist_item,0,255,cv2.NORM_MINMAX)
                hist=np.int32(np.around(hist_item))
                pts = np.column_stack((bins,hist))
                cv2.polylines(h,[pts],False,col)
            h=np.flipud(h)
            frameCopy[0:360, 0:360] = h.copy()

        cv2.rectangle(frameCopy, (horizontalPosition, verticalPosition), (horizontalPosition+sizeROI, verticalPosition+sizeROI), (255, 0, 0), 1)

        if self.lastFrame is not None:
            frame[:] = self.lastFrame

        self.lastFrame = frameCopy

if __name__ == "__main__":
    glutInit(sys.argv) # don't need this under Mac OS X
    app = QtGui.QApplication(sys.argv)
    w = MyMainWindow()
    w.show()
    
    sys.exit(app.exec_())
